#pragma once

#include "typedefs.h"
#include "vector.h"
#include "smatrix.h"
#include "matrix.h"

namespace g2 {

template <typename T>
class basic_translation;
template <typename T>
class basic_rigid;
template <typename T>
class basic_rigid_scale;
template <typename T>
class basic_affine;
template <typename T>
class basic_perspective;

//p2p_matcher like p2p_matcher finds best transformation (inside given class) 
//for matching two sets of points:
//t(Q) = P
//But each pair of points has associated weight also. The more the weight the more precise
//should the transformation match points of given pair.

template <class C>
class p2p_matcher;

template <class C>
class p2p_matcher_base : public accumulator_typedefs<typename C::element_type>
{
public:
	typedef C transform;
};

template <typename T>
class p2p_matcher< basic_translation<T> > : public p2p_matcher_base< basic_translation<T> >
{
public:
	p2p_matcher();
	void clear();
	void match(const vector & Q, const vector & P, element weight = 1);
  /// this function is made specifically for similarity with p2l_matcher
  void match(const vector & Q, const vector & P, const vector & , element weight = 1)
    { return match(Q, P, weight); }
	bool get_transform(transform & tr) const;
private:
	vector wT;
	element w;
};

template <typename T>
class p2p_matcher< basic_rigid<T> > : public p2p_matcher_base< basic_rigid<T> >
{
public:
	p2p_matcher();
	void clear();
	void match(const vector & Q, const vector & P, element weight = 1);
  void match(const vector & Q, const vector & P, const vector & , element weight = 1)
    { return match(Q, P, weight); }
	bool get_transform(transform & tr) const;
protected:
	vector wP, wQ;
	element scalar_product_wPQ, outer_product_wPQ;
	element w;
};

template <typename T>
class p2p_matcher< basic_rigid_scale<T> > : public p2p_matcher< basic_rigid<T> >
{
	typedef p2p_matcher< basic_rigid<T> > base;
public:
	typedef basic_rigid_scale<T> transform;
public:
	p2p_matcher();
	void clear();
	void match(const vector & Q, const vector & P, element weight = 1);
  void match(const vector & Q, const vector & P, const vector & , element weight = 1)
    { return match(Q, P, weight); }
	bool get_transform(transform & tr) const;
private:
	element w_squared_Q;
};

template <typename T>
class p2p_matcher< basic_affine<T> > : public p2p_matcher_base< basic_affine<T> >
{
public:
	p2p_matcher();
	void clear();
	void match(const vector & Q, const vector & P, element weight = 1);
  void match(const vector & Q, const vector & P, const vector & , element weight = 1)
    { return match(Q, P, weight); }
	bool get_transform(transform & tr) const;
private:
	vector wP, wQ;
  smatrix wQQ;
  matrix wPQ;
	element w;
};

template <typename T>
class p2p_matcher< basic_perspective<T> > : public p2p_matcher_base< basic_perspective<T> >
{
public:
	p2p_matcher();
	void clear();
	void match(const vector & Q, const vector & P, element weight = 1);
  void match(const vector & Q, const vector & P, const vector & , element weight = 1)
    { return match(Q, P, weight); }
	bool get_transform(transform & tr) const;
private:
	element w;
	vector p, q, r, phi1, phi2;
  smatrix Q, psi, F, G;
};

///////////////////////////
// inline implementation //
///////////////////////////

//
// translation p2p_matcher
//

template <typename T>
inline p2p_matcher< basic_translation<T> >::p2p_matcher()
	{ clear(); }

template <typename T>
inline void p2p_matcher< basic_translation<T> >::clear()
{
	wT.zero();
	w = 0;
}

template <typename T>
inline void p2p_matcher< basic_translation<T> >::match(const vector & Q, const vector & P, element weight)
{
	wT += weight * (P - Q);
	w += weight;
}

template <typename T>
inline bool p2p_matcher< basic_translation<T> >::get_transform(transform & tr) const
{
  if (w <= 0)
    return false;
  tr.d = (1 / w) * wT;
  return true;
}


//
// rigid p2p_matcher
//

template <typename T>
inline p2p_matcher< basic_rigid<T> >::p2p_matcher()
	{ clear(); }

template <typename T>
inline void p2p_matcher< basic_rigid<T> >::clear()
{
	wP = wQ.zero();
	scalar_product_wPQ = outer_product_wPQ = 0;
	w = 0;
}

template <typename T>
inline void p2p_matcher< basic_rigid<T> >::match(const vector & Q, const vector & P, element weight)
{
	w += weight;
	wP += weight * P;
	wQ += weight * Q;
	scalar_product_wPQ += weight * scalar_product(P,Q);
	outer_product_wPQ += weight * outer_product(P,Q);
}

template <typename T>
inline bool p2p_matcher< basic_rigid<T> >::get_transform(transform & tr) const
{
  if (w <= 0)
    return false;
	element rw = 1 / w;
	element scalar_product_pq = //~ +cos
		scalar_product_wPQ - scalar_product(wP, wQ) * rw;
	element outer_product_pq = //~ -sin
		outer_product_wPQ - outer_product(wP, wQ) * rw;

  element h = _hypot(scalar_product_pq, outer_product_pq);
  if (h <= 0)
    return false;
	element rh = 1 / h;
	element cos = scalar_product_pq * rh; 
	element sin = -outer_product_pq * rh;

  basic_rotation<element> r(cos, sin);
  tr.r = r;
  tr.d = rw * (wP - r(wQ));
	return true;
}

//
// rigid+scale p2p_matcher
//

template <typename T>
inline p2p_matcher< basic_rigid_scale<T> >::p2p_matcher()
	{ clear(); }

template <typename T>
inline void p2p_matcher< basic_rigid_scale<T> >::clear()
{
	base::clear();
	w_squared_Q = 0;
}

template <typename T>
inline void p2p_matcher< basic_rigid_scale<T> >::match(const vector & Q, const vector & P, element w)
{
	base::match(Q, P, w);
	w_squared_Q += w * sqr(Q);
}

template <typename T>
inline bool p2p_matcher< basic_rigid_scale<T> >::get_transform(transform & tr) const
{
  if (w <= 0)
    return false;
	element rw = 1 / w;
	element scalar_product_pq =
		scalar_product_wPQ - scalar_product(wP, wQ) * rw;
	element outer_product_pq =
		outer_product_wPQ - outer_product(wP, wQ) * rw;
	element squared_q = w_squared_Q - sqr(wQ) * rw;
  if (squared_q <= 0)
    return false;

  basic_stretched_rotation<element> r(
		scalar_product_pq / squared_q, 
		-outer_product_pq  / squared_q);
  tr.r = r;
  tr.d = rw * (wP - r(wQ));
	return true;
}


//
// affine p2p_matcher
//

template <typename T>
inline p2p_matcher< basic_affine<T> >::p2p_matcher()
	{ clear(); }

template <typename T>
inline void p2p_matcher< basic_affine<T> >::clear()
{
	wP = wQ.zero();
  wQQ.zero();
  wPQ.zero();
	w = 0;
}

template <typename T>
inline void p2p_matcher< basic_affine<T> >::match(const vector & Q, const vector & P, element weight)
{
	w += weight;
	wP += weight * P;
	wQ += weight * Q;
  wQQ += weight * tsquare(Q);
  wPQ += weight * tproduct(P, Q);
}

template <typename T>
inline bool p2p_matcher< basic_affine<T> >::get_transform(transform & tr) const
{
  if (w <= 0)
    return false;
	element rw = 1.0 / w;
  smatrix a = wQQ - rw * tsquare(wQ);
  element a_det = a.det();
  if (a_det <= 0)
    return false;

	matrix m = wPQ - rw * tproduct(wP, wQ);

  matrix r = m * matrix(a.inverse(a_det));
  tr.r = r;
  tr.d = rw * (wP - r(wQ));
  return true;
}


//
// perspective p2p_matcher
//

template <typename T>
inline p2p_matcher< basic_perspective<T> >::p2p_matcher()
	{ clear(); }

template <typename T>
inline void p2p_matcher< basic_perspective<T> >::clear()
{
	w = 0;
	p = q = r = phi1 = phi2.zero();
  Q = F = G = psi.zero();
}

template <typename T>
inline void p2p_matcher< basic_perspective<T> >::match(const vector & qi, const vector & pi, element wi)
{
	w += wi;

  p += wi * pi;
  vector wqi = wi * qi;
  q += wqi;
  element ppi = sqr(pi);
  r += ppi * wqi;
  phi1 += pi.x * wqi;
  phi2 += pi.y * wqi;

  smatrix Qi = wi * tsquare(qi);
  Q += Qi;
  psi += ppi * Qi;
  F += pi.x * Qi;
  G += pi.y * Qi;
}

template <typename T>
bool p2p_matcher< basic_perspective<T> >::get_transform(transform & tr) const
{
  if (w <= 0)
    return false;
	element rw = 1.0 / w;

  smatrix Qwave = Q - rw * tsquare(q);  
  element Qwave_det = Qwave.det();
  if (Qwave_det <= 0)
    return false;
  smatrix invQwave = Qwave.inverse(Qwave_det);

  matrix Fwave = matrix(F) - rw * tproduct(q, phi1);
  matrix Gwave = matrix(G) - rw * tproduct(q, phi2);
  vector f1 = phi1 - rw * p.x * q;
  vector f2 = phi2 - rw * p.y * q;

  smatrix x = psi
    - duplex_product(invQwave, Fwave) - duplex_product(invQwave, Gwave)
    - rw * (tsquare(phi1) + tsquare(phi2));
  element x_det = x.det();
  if (x_det <= 0)
    return false;
  smatrix invx = x.inverse(x_det);

  vector y = -r + (invQwave * f1) * Fwave + (invQwave * f2) * Gwave + rw * (p.x * phi1 + p.y * phi2);
  vector c = invx * y;

  vector a1 = invQwave * (Fwave * c + f1);
  vector a2 = invQwave * (Gwave * c + f2);

  vector b(
    rw * (scalar_product(phi1, c) - scalar_product(q, a1) + p.x),
    rw * (scalar_product(phi2, c) - scalar_product(q, a2) + p.y));

  tr.a = matrix(a1.x, a1.y, a2.x, a2.y);
  tr.b = b;
  tr.c = c;
  return true;
}

} //namespace g2
